//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
//_/_/
//_/_/  CADIA.Module.PitchTracker
//_/_/  Eric Nivel
//_/_/
//_/_/  Copyright(c)2007 Center for Analysis and Design of Intelligent Agents
//_/_/                   Reykjavik University
//_/_/                   All rights reserved
//_/_/
//_/_/                   http://cadia.ru.is/
//_/_/
//_/_/  Redistribution and use in source and binary forms, with or without
//_/_/  modification, is permitted provided that the following conditions 
//_/_/  are met:
//_/_/
//_/_/  - Redistributions of source code must retain the above copyright notice,
//_/_/    this list of conditions and the following disclaimer.
//_/_/
//_/_/  - Redistributions in binary form must reproduce the above copyright 
//_/_/    notice, this list of conditions and the following disclaimer in the 
//_/_/    documentation and/or other materials provided with the distribution.
//_/_/
//_/_/  - Neither the name of its copyright holders nor the names of its 
//_/_/    contributors may be used to endorse or promote products derived from 
//_/_/    this software without specific prior written permission.
//_/_/
//_/_/  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
//_/_/  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
//_/_/  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 
//_/_/  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
//_/_/  OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
//_/_/  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
//_/_/  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
//_/_/  PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
//_/_/  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
//_/_/  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
//_/_/  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//_/_/
//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/

#include "stdafx.h" //Added by Brynjar Reynisson when integrating into vocoder

#include	"WaveletPitchProcessor.h"

#include	<math.h>



WaveletPitchProcessor::WaveletPitchProcessor(const	unsigned	int	bufferSize,
 											 const	unsigned	int	samplingFrequency):bufferSize(bufferSize),
																																		samplingFrequency(samplingFrequency){

	maxIndices=new	unsigned	int[bufferSize];
	minIndices=new	unsigned	int[bufferSize];

	for(unsigned	int	i=0;i<ApproximationLevels;i++)
		signal[i]=new	double[bufferSize];

	differences=new	double[bufferSize*DifferenceLevels*2];
}

WaveletPitchProcessor::~WaveletPitchProcessor(){

	delete[]	maxIndices;
	delete[]	minIndices;

	for(unsigned	int	i=0;i<ApproximationLevels;i++)
		delete[]	signal[i];

	delete[]	differences;
}

void WaveletPitchProcessor::trackZeroCrossings(const short *buffer, PitchOut * dataOut)
{
	unsigned int count = 0;
	int state = 1;
	for(unsigned short i=0; i<bufferSize; i++)
	{
		if(buffer[i]<0 && state==1)
		{
			count++;
			state = -1;
		}
		else if(buffer[i]>0 && state==-1)
		{
			count++;
			state = 1;
		}
	}
	unsigned int frequency = (samplingFrequency/bufferSize)*count;
	if(frequency>300) frequency = 0;
	dataOut->pitch = frequency;
	if(frequency>0) dataOut->voiced = true;
	
}

void	WaveletPitchProcessor::trackPitch(const short	*buffer,
   			  			                  const	double				amplitudeThreshold,
										  const	double				noiseThreshold,
										  PitchOut	*dataOut){

	//	Visual algorithm. Perform several approximations of the signal (by reducing its resolution) - wavelets -
	//	and try to find similarities, i.e. points at the same intensity. The distance between them at a given level is called a mode.
	//	find an agreement at the different levels. If successfull we get the mode. As a denominator, this yields the base frequency, i.e. the pitch.

	//	This method is based on: http://online.physics.uiuc.edu/courses/phys199pom/NSF_REU_Reports/2005_reu/Real-Time_Time-Domain_Pitch_Tracking_Using_Wavelets_Final_Presentation.pdf




	//trackZeroCrossings(buffer, dataOut);
	//if(true)return;
	double	average=0;
	double	signalMax=-100000;
	double	signalMin=100000;

	dataOut->signal_energy=0;
	for(unsigned	i=0;i<bufferSize;i++){

		signal[0][i]=buffer[i];

		average+=buffer[i];
		if(signalMax<buffer[i])
			signalMax=buffer[i];
		if(signalMin>buffer[i])
			signalMin=buffer[i];

		dataOut->signal_energy+=abs(buffer[i]);
	}
	dataOut->signal_energy/=bufferSize;

	average/=bufferSize;

	dataOut->signal_high=abs(abs(signalMax)-abs(average));
	dataOut->signal_low=abs(abs(average)-abs(signalMin));

	double logSignal = log(0.5*abs(dataOut->signal_high+dataOut->signal_low));
	if(logSignal<noiseThreshold){

		dataOut->voiced=false;
		return;
	}else
		dataOut->voiced=true;

	//	DC offset: center around the average and discard extreme values
	double	maxThreshold=amplitudeThreshold*(signalMax-average)+average;
	double	minThreshold=amplitudeThreshold*(signalMin-average)+average;

	maxCount[0]=0;
	minCount[0]=0;

	mode[0]=0;

	for(unsigned	int	two_pow_i=2,i=1;i<ApproximationLevels;i++,two_pow_i*=2){

		unsigned	int	span=bufferSize/two_pow_i;	//	of the approximation

		// approximation by reducing the resolution by 2
		for(unsigned	int	k=0;k<span;k++)
			signal[i][k]=(signal[i-1][2*k]+signal[i-1][2*k+1])/2;

		unsigned int f = (unsigned int) (floor(((double)samplingFrequency)/(MaxFrequency*two_pow_i)));
		unsigned int minDist = f>1?f:1;
		
		maxCount[i]=0;
		minCount[i]=0;

		for(unsigned	int	k=0;k<bufferSize;k++){

			maxIndices[k]=0;
			minIndices[k]=0;
		}

		int	transitionSign=0;	// denotes a change in the derivative of the signal
		if(signal[i][1]-signal[i][0]>0)
			transitionSign=1;
		else
			transitionSign=-1;

		bool	extremeAvailable=true;	// after crossing a zero, indicates if an extremum can be found
		unsigned	int	samplesToGo=0;	// number of samples to reach before potentially reaching a meaningful extremum

		for(unsigned	int	j=1;j<span;j++){

			double	signalDelta=signal[i][j]-signal[i][j-1];

			if (transitionSign>=0	&&	signalDelta<0){

				if(signal[i][j-1]>=maxThreshold	&&	extremeAvailable	&&	!samplesToGo){

					maxCount[i]++;	//	values in [0, ..., span-1]
					maxIndices[maxCount[i]-1]=j-1;
					extremeAvailable=false;
					samplesToGo=minDist;
				}
				transitionSign=-1;
			}else	if (transitionSign<=0	&&	signalDelta>0){

				if(signal[i][j-1]<=minThreshold	&&	extremeAvailable	&&	!samplesToGo){

					minCount[i]++;
					minIndices[minCount[i]-1]=j-1;
					extremeAvailable=false;
					samplesToGo=minDist;
				}
				transitionSign=1;
			}

			if((signal[i][j]<=average && signal[i][j-1]>average) || (signal[i][j]>=average && signal[i][j-1]<average))	//	zero crossing
				extremeAvailable=true;

			if(samplesToGo)
				samplesToGo=samplesToGo-1;
		}

		if(maxCount[i]>=2 && minCount[i]>=2){	//	compute the mode distance between peaks at each level of approximation

			unsigned	int	dCount=0;	//	compute the differences at differenceRadius distances

			for(int	d=1;d<=DifferenceLevels;d++){
				
				int	k;
				for(k=0;k<maxCount[i]-d;k++)
					differences[dCount++]=abs((double)(maxIndices[k+d]-maxIndices[k]));
				for(k=0;k<minCount[i]-d;k++)
					differences[dCount++]=abs((double)(minIndices[k+d]-minIndices[k]));
			}

			// a mode is the distance in time from one point to another within a similar shape, at the current approximation level
			//	set the mode as the center of the differences
			unsigned	int	candidateCount=1;
			unsigned	int	occurences;	//	number of occurences of distance j being within minimumDistance samples of another distance
			mode[i]=0;

			for(unsigned	int	j=0;j<dCount;j++){

				occurences=0;
				for(unsigned	int	k=0;k<dCount;k++){

					if(abs((double)(differences[k]-differences[j]))<=minDist)
						occurences++;
				}
				
				//	when there's more occurences than candidates, we have potentially a new mode (will be confirmed if it corresponds to other modes at different approximation levels)
				if (occurences>=candidateCount	&&	occurences>floor((double)span/differences[j])/4){

					if (occurences==candidateCount){

						if(differences[j]>1.95*mode[i]	&&	differences[j]<2.05*mode[i])	//	5 percent tolerance: this is a visual algorithm after all!
							mode[i]=differences[j];
					}else{

						candidateCount=occurences;
						mode[i]=differences[j];
					}
				}
			}

			if (mode[i]){	//	if we have a potential new mode, set it as the average of the candidates

				unsigned	int	hits=0;
				double	m=0;
				for(unsigned	int	k=0;k<dCount;k++){

					if(abs((double)(differences[k]-mode[i]))<=minDist){

						hits++;
						m+=differences[k];
					}
				}
				if(hits>0)
					mode[i]=m/hits;
			}

			if(mode[i-1]	&&	maxCount[i-1]>=2	&&	minCount[i-1]>=2){	//	find out if the modes are within the span of eachother
				
				if (abs(mode[i-1]-2*mode[i])<=minDist){	//	success in finding a pitch when the modes are within a sample of one another

					dataOut->pitch=((double)samplingFrequency)/(mode[i-1]*two_pow_i);
					dataOut->level=i;
					return;
				}
			}
		}
	}

	dataOut->pitch=0;
	dataOut->level=ApproximationLevels;
}